
extern volatile float x_VAL, y_VAL;
extern volatile float panAngle, tiltAngle, zeroRange;

#include <stdio.h>
#include <string.h>
#include "UART.h"
#include "RangeFinder.h"
#include "Servos.h"
#include <math.h>

#define MESSAGE_TYPE_LENGTH 5
#define MESSAGE_LENGTH 100
#define CHECKSUM_LENGTH 5

/* This should probably be called when the UART ISR has completed receiving the sentence */
/* Do this by setting a 'transmission received' flag in the ISR when a \r\n sequence is read */
/*  and poll for this flag in the main routine */


int parseString(char * inputString){


	unsigned int stringStart, i,j=0, completeSentence=0;
	float range,pan,tilt;
	char messageType[MESSAGE_TYPE_LENGTH], message[MESSAGE_LENGTH],checksum[CHECKSUM_LENGTH];
	char returnString[20];

	for(i=0;i<=MESSAGE_TYPE_LENGTH;i++){
		messageType[i]='\0';
	}

	for(i=0;i<=MESSAGE_LENGTH;i++){
		message[i]='\0';
	}

	for(i=0;i<=CHECKSUM_LENGTH;i++){
		checksum[i]='\0';
	}


	for(stringStart=0;stringStart<strlen(inputString);stringStart++){
		if(inputString[stringStart] == '$'){
			for(i=1;i<=5;i++){
				messageType[i-1] = inputString[stringStart + i];
			}

			i = stringStart + 7;
			while(!(inputString[i] == '*') && !(inputString[i] == 10) && j<MESSAGE_LENGTH){
				message[j++]=inputString[i++];
			}

			i++;
			j=0;

			while(!(inputString[i] == 10) && !(inputString[i] == 13) && j<=strlen(checksum)){
				checksum[j++]=inputString[i++];
			}
			completeSentence = 1;
			break;
		}
	}


	if(!strcmp(messageType,"LTCON")){
		sscanf(message,"%f,%f",&x_VAL,&y_VAL);

		tiltAngle = atan(x_VAL/zeroRange)*57.3;		// ...*180/3.14
		panAngle = atan(y_VAL/zeroRange)*57.3;

		updateServos(panAngle, tiltAngle);
	}

	if(!strcmp(messageType,"LTANG")){
		sscanf(message,"%f,%f",&pan,&tilt);
		updateServos(pan,tilt);
	}

	if(!strcmp(messageType,"LTRAN")){
		range = findRange();
		sprintf(returnString,"\r\n$LTRAN,%f*00\r\n",range);

		UART4_Send(returnString, strlen(returnString));
	}

	if(!strcmp(messageType,"LTZER")){
		range = findRangeAtZero();
		sprintf(returnString,"\r\n$LTZER,%f*00\r\n",range);

		UART4_Send(returnString, strlen(returnString));
	}

	if(!strcmp(messageType,"LTHRT")){
		sprintf(returnString,"\r\n$LTHRT,%d*00\r\n",1);
		UART4_Send(returnString, strlen(returnString));
	}


	return completeSentence;
}
